package AStar;

import java.util.ArrayList;
import java.util.Collection;
import java.util.List;

import nav.AStar;
import nav.Crossing;
import nav.Link;
import nav.Topology;

public class AStarImpl implements AStar {

	private Topology topology;
	
	public AStarImpl(Topology topology){
		this.topology = topology;
	}
		
	@Override
	public List<Crossing> getPath(Crossing start, Crossing goal) throws AStarPathNotFoundException {
		AStarNode startNode = new AStarNodeImpl(start);
		AStarNode goalNode = new AStarNodeImpl(goal);
		startNode.setCostsFromNodeToGoal(calcDistance(startNode,goalNode));
		startNode.setCostsFromStartToGoal(startNode.getCostsFromNodeToGoal());
		startNode.setCostsFromStartToNode(0);
		List<AStarNode> openList = new ArrayList<AStarNode>();
		List<AStarNode> closedList = new ArrayList<AStarNode>();
		openList.add(startNode);
		
		while(!openList.isEmpty()){
			AStarNode currentNode = getLowestNode(openList);
			if(currentNode.getCrossing() == goalNode.getCrossing()){
				goalNode = currentNode;
				return getPath(startNode,goalNode);
			}
			expandNode(currentNode,startNode,goalNode,closedList,openList);
			closedList.add(currentNode);
		}		
		
		throw new AStarPathNotFoundException(start,goal);
	}
	
	private AStarNode getLowestNode(List<AStarNode> openList) {
		if(openList.size() == 0){
			return null;
		}
		AStarNode minNode = openList.get(0);
		for(AStarNode node : openList){
			if(minNode.getCostsFromStartToGoal() > node.getCostsFromStartToGoal()){
				minNode = node;
			}
		}
		openList.remove(minNode);
		return minNode;
	}

	private void expandNode(AStarNode currentNode, AStarNode startNode, AStarNode goalNode, List<AStarNode> closedList, List<AStarNode> openList) {
		AStarNode[] neighbors = getNeighbors(currentNode,startNode,goalNode);
		for(AStarNode neighbor : neighbors){			
			if(collectionContainsNode(closedList,neighbor)){
				continue;
			}
			double costsFromStartToNode = currentNode.getCostsFromStartToNode() + calcDistance(currentNode, neighbor);
			double costsFromStartToGoal = costsFromStartToNode + neighbor.getCostsFromNodeToGoal();
			if(collectionContainsNode(openList,neighbor) && costsFromStartToGoal > neighbor.getCostsFromStartToGoal()){
				continue;
			}
			neighbor.setSuccessor(currentNode);
			neighbor.setCostsFromStartToNode(costsFromStartToNode);
			neighbor.setCostsFromStartToGoal(costsFromStartToGoal);
			addNodeToOpenList(openList,neighbor);
		}
	}

	private void addNodeToOpenList(List<AStarNode> openList, AStarNode neighbor) {
//		for(int i=0;i<openList.size();i++){
//			if(openList.get(i).getCostsFromStartToGoal() > neighbor.getCostsFromStartToGoal()){
//				openList.add(i, neighbor);
//				return;
//			}
//		}
		openList.add(neighbor);
	}

	private boolean collectionContainsNode(Collection<AStarNode> collection, AStarNode node) {
		for(AStarNode aStarNode : collection){
			if(aStarNode.getCrossing() == node.getCrossing()){
				return true;
			}
			if(aStarNode == node){
				return true;
			}
		}
		return false;
	}

	private double calcDistance(AStarNode from, AStarNode to) {
		return from.getCrossing().getCoordinate().distanceTo(to.getCrossing().getCoordinate());
	}

	private AStarNode[] getNeighbors(AStarNode node, AStarNode startNode, AStarNode goalNode) {
		Crossing[] crossings = getCrossings(node);
		List<AStarNode> nodes = new ArrayList<AStarNode>();
		for(Crossing crossing : crossings){
			AStarNode aStarNode = new AStarNodeImpl(crossing);
			aStarNode.setCostsFromNodeToGoal(calcDistance(node,goalNode));
			aStarNode.setCostsFromStartToNode(calcDistanceFromStartToNode(node));
			aStarNode.setCostsFromStartToGoal(aStarNode.getCostsFromStartToNode() + aStarNode.getCostsFromNodeToGoal());
			nodes.add(aStarNode);
		}
		return nodes.toArray(new AStarNode[]{});
	}
	
	private double calcDistanceFromStartToNode(AStarNode node) {
		AStarNode n = node.getSuccessorNode();
		if(n == null){
			return 0;
		}
		for(Link link : n.getCrossing().getLinks()){
			if(link.crossingTo() == node.getCrossing().getID()){
				return n.getCostsFromStartToNode() + link.length();
			}
		}
		return 0;
	}

	private Crossing[] getCrossings(AStarNode node){
		Crossing crossing = node.getCrossing();
		List<Link> links = crossing.getLinks();
		List<Crossing> crossings = new ArrayList<Crossing>();
		for(Link link : links){
			crossings.add(topology.getCrossing(link.crossingTo()));
		}
		return crossings.toArray(new Crossing[]{});
	}

	private List<Crossing> getPath(AStarNode start, AStarNode goal){
		List<Crossing> cross = new ArrayList<Crossing>();
		cross.add(goal.getCrossing());
		AStarNode successor = goal.getSuccessorNode();
		while(successor!=null){
			cross.add(0, successor.getCrossing());
			successor = successor.getSuccessorNode();
		}		
		return cross;
	}
}
